#include "../include/config/config.h"
#include "opencv2/opencv.hpp"
#include "ros/ros.h"

static fyt_msg::CameraParam cameraParam;
static fyt_msg::RuneParam runeParam;
static fyt_msg::DetectorParam detectorParam;
static fyt_msg::SerialParam serialParam;

void loadCameraParam(const std::string &filename){
    cv::FileStorage fs(filename,cv::FileStorage::READ);
    fs["source"] >> cameraParam.source;
    fs["resolution_width"] >> cameraParam.resolution_width;
    fs["resolution_height"] >> cameraParam.resolution_height;
    fs["auto_white_balance"] >> cameraParam.auto_white_balance;
    fs["frame_rate"] >> cameraParam.frame_rate;
    fs["auto_explosure"] >> cameraParam.auto_explosure;
    fs["explosure_time"] >> cameraParam.explosure_time;
    fs["explosure_time_1"] >> cameraParam.explosure_time_1;
    fs["explosure_time_2"] >> cameraParam.explosure_time_2;
    
    int camera_type;
    fs["camera_type"] >> camera_type;
    if(camera_type == 8){
        fs["fx_8"] >> cameraParam.fx;
        fs["fy_8"] >> cameraParam.fy;
    } 
    else if(camera_type == 12){
        fs["fx_12"] >> cameraParam.fx;
        fs["fy_12"] >> cameraParam.fy;
    }
    else if(camera_type == 16){
        fs["fx_16"] >> cameraParam.fx;
        fs["fy_16"] >> cameraParam.fy;
    }
    else{
        fs["fx_6"] >> cameraParam.fx;
        fs["fy_6"] >> cameraParam.fy;
    }
    fs.release();
}

void loadRuneParam(const std::string &filename){
    cv::FileStorage fs(filename,cv::FileStorage::READ);
    fs["frame_interval_s"] >> runeParam.frame_interval_s;
    fs["predict_time"] >> runeParam.predict_time;
    
    fs["proc_mode"] >> runeParam.proc_mode;
    fs["blue_thresh"] >> runeParam.blue_thresh;
    fs["red_thresh"] >> runeParam.red_thresh;
    fs["ed_times"] >> runeParam.ed_times;
    
    fs["r_max_ratio"] >> runeParam.r_max_ratio;
    fs["r_min_area"] >> runeParam.r_min_area;
    fs["r_max_area"] >> runeParam.r_max_area;
    
    fs["leaf_min_area"] >> runeParam.leaf_min_area;
    fs["leaf_max_area"] >> runeParam.leaf_max_area;
    fs["leaf_min_ratio"] >> runeParam.leaf_min_ratio;
    fs["leaf_max_ratio"] >> runeParam.leaf_max_ratio;

    fs["armor_min_area"] >> runeParam.armor_min_area;
    fs["armor_max_area"] >> runeParam.armor_max_area;
    fs["armor_min_ratio"] >> runeParam.armor_min_ratio;
    fs["armor_max_ratio"] >> runeParam.armor_max_ratio;

    fs["R2armor_min_dist"] >> runeParam.R2armor_min_dist;
    fs["R2armor_max_dist"] >> runeParam.R2armor_max_dist;
    
    fs["compensation_1"] >> runeParam.compensation_1;
    fs["compensation_2"] >> runeParam.compensation_2;
    fs["compensation_3"] >> runeParam.compensation_3;
    fs["compensation_4"] >> runeParam.compensation_4;
    fs.release();
}

void loadDetectorParam(const std::string &filename){
    cv::FileStorage fs(filename,cv::FileStorage::READ);
    fs["debug"] >> detectorParam.debug;
    fs["device"] >> detectorParam.device;
    fs["video_writer"] >> detectorParam.video_writer;
    fs["calc_time"] >>detectorParam.calc_time;
    fs["input_size"] >> detectorParam.input_size;
    fs["model_path"] >> detectorParam.model_path;
    fs["class_name_path"] >> detectorParam.class_name_path;
    fs["confThr"] >> detectorParam.confThr;
    fs["nmsThr"] >> detectorParam.nmsThr;
    fs["port_name"] >>detectorParam.port_name;
    fs["use_serial"] >> detectorParam.use_serial;
    fs.release();
}

void loadSerialParam(const std::string &filename){

}

int main(int argc,char **argv){
    ros::init(argc,argv,"config_node");
    ros::NodeHandle n;

    ros::Publisher pub_cameraParam=n.advertise<fyt_msg::CameraParam>("config_camera",1);
    ros::Publisher pub_runeParam=n.advertise<fyt_msg::RuneParam>("config_rune",1);
    ros::Publisher pub_detectorParam=n.advertise<fyt_msg::DetectorParam>("config_detector",1);
    // ros::Publisher pub_serialParam=n.advertise<fyt_msg::SerialParam>("config_serial",1);
    
    while(ros::ok()){

        loadCameraParam("config_file/CameraParam.yml");
        loadRuneParam("config_file/RuneParam.yml");
        loadDetectorParam("config_file/DetectorParam.yml");
        loadSerialParam("config_file/SerialParam.yml");
        
        pub_cameraParam.publish(cameraParam);
        pub_runeParam.publish(runeParam);

        detectorParam.camera_fx=cameraParam.fx;
        detectorParam.camera_fy=cameraParam.fy;
        detectorParam.width=cameraParam.resolution_width;
        detectorParam.height=cameraParam.resolution_height;

        pub_detectorParam.publish(detectorParam);
        // pub_serialParam.publish(serialParam);

        sleep(3);
    }
}
